#include "observation_buffer.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_sensor_msgs/tf2_sensor_msgs.h>
#include <sensor_msgs/point_cloud2_iterator.h>

namespace costmap_2d{

ObservationBuffer::ObservationBuffer(string topic_name, double observation_keep_time,
    double obstacle_range, double raytrace_range, tf2_ros::Buffer& tf2_buffer, 
    string global_frame, double tf_tolerance):
    tf2_buffer_(tf2_buffer), observation_keep_time_(observation_keep_time),
    last_updated_(ros::Time::now()), global_frame_(global_frame), topic_name_(topic_name),
    obstacle_range_(obstacle_range), raytrace_range_(raytrace_range), tf_tolerance_(tf_tolerance){
}

ObservationBuffer::~ObservationBuffer(){
}


void ObservationBuffer::bufferCloud(const sensor_msgs::PointCloud2& cloud){
    geometry_msgs::PointStamped global_origin;
    observation_list_.push_front(Observation());
    string origin_frame = cloud.header.frame_id;

    try{
        geometry_msgs::PointStamped local_origin;
        local_origin.header.stamp = cloud.header.stamp;
        local_origin.header.frame_id = origin_frame;
        local_origin.point.x = 0;
        local_origin.point.y = 0;
        local_origin.point.z = 0;
        tf2_buffer_.transform(local_origin, global_origin, global_frame_);
        tf2::convert(global_origin.point, observation_list_.front().origin_);

        observation_list_.front().raytrace_range_ = raytrace_range_;
        observation_list_.front().obstacle_range_ = obstacle_range_;
        
        sensor_msgs::PointCloud2 global_frame_cloud;

        // transform the point cloud
        tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_);
        global_frame_cloud.header.stamp = cloud.header.stamp;

        sensor_msgs::PointCloud2& observation_cloud = *(observation_list_.front().cloud_);
        observation_cloud = global_frame_cloud;
        
    }
    catch (TransformException& ex){
        observation_list_.pop_front();
        ROS_ERROR("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", origin_frame.c_str(),
                    cloud.header.frame_id.c_str(), ex.what());
        return;
    }

    last_updated_ = ros::Time::now();   
    purgeStaleObservations();
}


void ObservationBuffer::getObservations(vector<Observation>& observations){
    purgeStaleObservations();

    list<Observation>::iterator obs_it;
    for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it){
        observations.push_back(*obs_it);
    }
}

void ObservationBuffer::purgeStaleObservations(){
    if(!observation_list_.empty()){
        list<Observation>::iterator obs_it = observation_list_.begin();
        if(observation_keep_time_ == ros::Duration(0.0)){
            observation_list_.erase(++obs_it, observation_list_.end());
            return;
        }

        for (obs_it = observation_list_.begin(); obs_it != observation_list_.end(); ++obs_it){
            Observation& obs = *obs_it; 
            if ((last_updated_ - obs.cloud_->header.stamp) > observation_keep_time_){
                observation_list_.erase(obs_it, observation_list_.end());
                return;
            }
        }
    }
}





};  // namespace costmap_2d
